import numpy as np
import math
import sys
sys.path.append('../CWJ')
from Source.CWJ.gdpz import intersection_to_point, intersection_to_mian
from Source.CWJ.gdzbzh import pzjc
from Source.CWJ.qyhf import check

def qiudian2(pi, block, long):
    x = block[1][0] - block[0][1]
    y = block[1][1] - block[0][1]
    if x >= y:
        newpoint = [[pi[1][0], pi[1][1] - long, pi[1][2]], [pi[1][0], pi[1][1] + long, pi[1][2]]]
    else:
        newpoint = [[pi[1][0] - long, pi[1][1], pi[1][2]], [pi[1][0] + long, pi[1][1], pi[1][2]]]
    return newpoint

def qiudian(point1, point2, long, angle):
    p = np.zeros(2)
    for i in range(2):
        p[i] = point2[i] - point1[i]
    a = (p[0] ** 2 + p[1] ** 2) ** 0.5
    if p[0] != 0:
        k = p[1] / p[0]  # 直线1的斜率
    if angle == 1:  # 两直线垂直
        goal = np.zeros(3)
        goal1 = np.zeros(3)
        goal[0] = point2[0] + long * (p[1] / a)
        goal1[0] = point2[0] - long * (p[1] / a)
        goal[1] = point2[1] - long * (p[0] / a)
        goal1[1] = point2[1] + long * (p[0] / a)
        goal[2] = point2[2]
        goal1[2] = point2[2]
        # print(goal,goal1)
        return goal, goal1
    if angle == 2:  # 夹角为45度时
        if p[0] == 0:
            k = 0
        goal = np.zeros(3)
        goal1 = np.zeros(3)
        goal2 = np.zeros(3)
        goal3 = np.zeros(3)
        if k == 1 or k == -1:
            # 求出x，y坐标
            goal[0] = point2[0] - long
            goal[1] = point2[1]
            goal1[0] = point2[0] + long
            goal1[1] = point2[1]
            goal2[0] = point2[0]
            goal2[1] = point2[1] - long
            goal3[0] = point2[0]
            goal3[1] = point2[1] + long
        else:

            k1 = (1 + k) / (1 - k)
            a = (k1 ** 2 + 1) ** 0.5
            goal[0] = point2[0] + long * (1 / a)
            goal1[0] = point2[0] - long * (1 / a)
            goal[1] = point2[1] - long * (k1 / a)
            goal1[1] = point2[1] + long * (k1 / a)
            k2 = (k - 1) / (1 + k)
            b = (k2 ** 2 + 1) ** 0.5
            goal2[0] = point2[0] + long * (1 / b)
            goal3[0] = point2[0] - long * (1 / b)
            goal2[1] = point2[1] - long * (k2 / b)
            goal3[1] = point2[1] + long * (k2 / b)
        goal[2] = point2[2]
        goal1[2] = point2[2]
        goal2[2] = point2[2]
        goal3[2] = point2[2]
        # print(goal,goal1,goal2,goal3)
        return [goal, goal1, goal2, goal3]

def zuoyou(coor, stru):
    cx = []
    cy = []
    sx = []
    sy = []
    for i in range(0, len(coor)):
        cx.append(coor[i][0])
        cy.append(coor[i][1])
    for j in range(0, len(stru)):
        sx.append(stru[j][0])
        sy.append(stru[j][1])
    cmax = [max(cx), max(cy)]
    smax = [max(sx), max(sy)]
    cmin = [min(cx), min(cy)]
    smin = [min(sx), min(sy)]
    dis1 = math.sqrt((cmax[0] - smax[0]) ** 2 + (cmax[1] - smax[1]) ** 2)
    dis2 = math.sqrt((cmin[0] - smin[0]) ** 2 + (cmin[1] - smin[1]) ** 2)
    return dis2 - dis1

# 输入碰撞坐标和管道流，输出碰撞的平均坐标
def Average_value(coor, pi):
    # print(coor)
    index = len(coor)
    x = 0
    y = 0
    z = 0
    for i in range(index):
        x = x + coor[i][0]
        y = y + coor[i][1]
        z = z + coor[i][2]
    x = x / index
    y = y / index
    z = z / index
    xyz = [x, y, z]
    if pi[3] >= 600:
        redis = pi[3] * 3
    else:
        redis = pi[3] * 2  # 回退的长度为2个障碍物的宽度
    step = [redis / x, redis / y, redis / z]  # 步长
    for j in range(0, 3):
        if pi[1][j] - pi[0][j] == 0:
            xyz[j] = pi[0][j]
        else:
            if pi[1][j] >= pi[0][j]:
                xyz[j] = (xyz[j] * (1 - step[j]))
            else:
                xyz[j] = (xyz[j] * (1 + step[j]))
    xy = [x - xyz[0], y - xyz[1], z - xyz[2]]
    return [xyz, xy]


# 上下型障碍物
# 输入管道流和碰撞坐标，输出避障后的管道流
# pi: 管道流
# stru: 上下型障碍物表达式（与管道表示相同
# temp: 上下型障碍物的类型
# list1: 附近障碍物的列表
def Avoid_crossbeam(pi, stru, block_list, temp, BlockBase):
    z1 = pi[1][2]
    coor = intersection_to_point(pi, stru)  # 碰撞坐标
    m = 0
    if coor == []:
        # print('检测出碰撞，但碰撞坐标为空')
        final_pi = pi
    else:
        xyz = Average_value(coor, pi)
        high = stru[3]  # 障碍物的高度
        # lenp：管道回退的距离
        # d1: 回退的距离
        lenp = math.sqrt((xyz[0][0] - pi[0][0]) ** 2 + (xyz[0][1] - pi[0][1]) ** 2)
        d1 = math.sqrt(xyz[1][0] ** 2 + xyz[1][1] ** 2)
        # 用管道回退后剩下的长度除以回退的距离，计算出可以循环的次数
        if d1 == 0:
            final_pi = pi
        else:
            j1 = int(lenp / d1)
            if j1 <= 2:
                final_pi = pi
            else:
                if temp == 'Frame':  # 如果是横梁，向下
                    for j in range(0, j1 - 2):
                        new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                        for i in range(1, 9):
                            new_xyz = [new_pi[1][0], new_pi[1][1], new_pi[1][2] - 1 * i * high]
                            pi2 = [new_pi[1], new_xyz, pi[2], pi[3]]  # 向下的直管
                            a = pzjc([pi2], [stru])
                            if a == [[]]:
                                pi3 = [pi2[1], [pi[1][0], pi[1][1], pi2[1][2]], pi[2], pi[3]]  # 直管接下的横管（与障碍物平行）
                                b = pzjc([pi3], [stru])
                                if b == [[]]:
                                    final_pi = [new_pi, pi2, pi3]
                                    m = 1
                                    for z in range(0, len(block_list)):
                                        d = pzjc(final_pi, [block_list[z]])
                                        e = []
                                        for o in range(0, len(final_pi)):
                                            e.append([])
                                        if d != e:
                                            final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                                    f = pzjc(final_pi, block_list)
                                    if len(block_list) == 0:
                                        break
                                    else:
                                        g = []
                                        for o in range(0, len(final_pi)):
                                            g.append([])
                                        if f == g:  # 绕行完成
                                            break
                                if i == 8:
                                    m = 2
                                    final_pi = pi
                                    break
                            else:
                                xyz1 = Average_value([xyz[0]], pi)
                                xyz[0] = xyz1[0]
                                if j == j1:
                                    final_pi = pi
                                break

                        if m == 1 or m == 2 or m == 3:
                            break

                else:  # 否则是管道或其他
                    # 向上
                    for j in range(0, j1 - 2):
                        new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                        for i in range(1, 9):
                            new_xyz = [new_pi[1][0], new_pi[1][1], new_pi[1][2] + 1 * i * high]
                            pi2 = [new_pi[1], new_xyz, pi[2], pi[3]]  # 向上的直管
                            a = pzjc([pi2], [stru])
                            if a == [[]]:
                                pi3 = [pi2[1], [pi[1][0], pi[1][1], pi2[1][2]], pi[2], pi[3]]  # 直管接下的横管（与障碍物平行）
                                b = pzjc([pi3], [stru])
                                if b == [[]]:
                                    final_pi = [new_pi, pi2, pi3]
                                    m = 1
                                    for z in range(0, len(block_list)):
                                        d = pzjc(final_pi, [block_list[z]])
                                        e = []
                                        for o in range(0, len(final_pi)):
                                            e.append([])
                                        if d != e:
                                            final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                                    f = pzjc(final_pi, block_list)
                                    if len(block_list) == 0:
                                        break
                                    else:
                                        g = []
                                        for o in range(0, len(final_pi)):
                                            g.append([])
                                        if f == g:  # 绕行完成
                                            break
                                if i == 8:
                                    m = 2
                                    final_pi = Avoid_crossbeam(pi, stru, block_list, 'Frame', BlockBase)
    # 我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释
                                    # 测finalpi是不是2维（左边三个括号
                                    final_pi = [final_pi[0]]
                                    break
                            else:
                                xyz1 = Average_value([xyz[0]], pi)
                                xyz[0] = xyz1[0]
                                if j == j1:
                                    final_pi = pi
                                break

                        if m == 1 or m == 2 or m == 3:
                            break
    if final_pi == pi:
        z2 = z1
    else:
        len_finalpi = len(final_pi)
        z2 = final_pi[len_finalpi - 1][1][2]
    rectify = [0, 0, z2 - z1]
    # print('f', final_pi)
    return [final_pi, rectify]

# 左右型障碍物
# 输入管道流，碰撞坐标和障碍物类型，输出避障后的管道流
# coor：碰撞点坐标
# pi: 管道流
# column: 上下型障碍物表达式（与管道流表达相同
# stru: 障碍物产生碰撞的面上的四点坐标
def Avoid_column(pi, column, block_list, BlockBase, temp=0):
    coor = intersection_to_point(pi, column)
    # print(coor)
    stru = intersection_to_mian(pi, column)
    index = 0
    m = 0
    n = 0
    wide = max(column[2], column[3])  # 障碍物宽度
    if coor == []:
        # print('检测出碰撞，但碰撞坐标为空')
        final_pi = pi
    else:
        xyz = Average_value(coor, pi)
        # 管道终点 距离 碰撞平均坐标的长度
        lenp = math.sqrt((xyz[0][0] - pi[0][0]) ** 2 + (xyz[0][1] - pi[0][1]) ** 2 + (xyz[0][2] - pi[0][2]) ** 2)
        d1 = math.sqrt(xyz[1][0] ** 2 + xyz[1][1] ** 2 + xyz[1][2] ** 2)
        # 用管道回退后剩下的长度除以回退的距离，计算出可以循环的次数
        if d1 != 0:
            j1 = int(lenp / d1)
            if j1 <= 2:
                index = 1
            else:
                if temp == 0:
                    dis = zuoyou(coor, stru)
                else:
                    dis = temp
                    # #point1管道1起点，point2管道1终点，long管道2长度，angle夹角angle=1为垂直，angle=2为45度夹角

                if dis >= 0:  # 向右拐
                    # ①
                    for j in range(0, j1 - 2):
                        len_pi = math.sqrt((pi[1][0] - xyz[0][0]) ** 2 + (pi[1][1] - xyz[0][1]) ** 2 + (pi[1][2] - xyz[0][2]) ** 2)  # 管道终点到回退点的距离
                        new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                        # 横向增加的长度取8个wide
                        # ②
                        for i in range(1, 9):
                            a = qiudian(new_pi[0], new_pi[1], (i / 1) * wide, 1)
                            list11 = a[0].tolist()
                            pi2 = [new_pi[1], list11, pi[2], pi[3]]  # 向右拐（y方向）
                            test1 = pzjc([pi2], [column])
                            # ③
                            len_pi3 = 2 * d1 * (j + 1) + wide
                            if test1 == [[]]:
                                b = qiudian(pi2[0], pi2[1], len_pi3, 1)
                                list2 = b[1].tolist()
                                pi3 = [pi2[1], list2, pi[2], pi[3]]
                                c = qiudian(pi3[0], pi3[1], (i / 1) * wide, 1)
                                list22 = c[1].tolist()
                                pi4 = [pi3[1], list22, pi[2], pi[3]]
                                pi5 = [pi4[1], pi[1], pi[2], pi[3]]
                                a1 = pzjc([pi3, pi4, pi5], [column])
                                # ④
                                if a1 == [[], [], []]:
                                    m = 1
                                    final_pi = [new_pi, pi2, pi3, pi4, pi5]
                                    # 遍历附近所有障碍物
                                    # ⑤
                                    for z in range(0, len(block_list)):
                                        d = pzjc(final_pi, [block_list[z]])
                                        e = []
                                        for o in range(0, len(final_pi)):
                                            e.append([])
                                        if d == e:
                                            n = n + 1
                                        else:
                                            continue
                                    if n == len(block_list):  # 绕行完成
                                        break
                                    else:  # 向右绕行有障碍物，尝试向左
                                        m = 3
                                        if temp == 0:    # 第一次调用，尝试向左
                                            final_pi = Avoid_column(pi, column, block_list, BlockBase, -1)
                                            final_pi = [final_pi[0]]
    # 我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释
                                        # 测finalpi是不是2维（左边三个括号
                                            f = pzjc(final_pi, block_list)
                                            if len(block_list) == 0:
                                                break
                                            else:
                                                g = []
                                                for o in range(0, len(final_pi)):
                                                    g.append([])
                                                if f == g:
                                                    break
                                        else:            # 第n次调用，重新计算避障
                                            final_pi = Pi_bypass(final_pi, block_list, BlockBase)
                                # ④
                                else:
                                    if i == 8:
                                        m = 2
                                        index = 1
                                    continue
                            # ③
                            else:
                                xyz1 = Average_value([xyz[0]], pi)
                                xyz[0] = xyz1[0]
                                if j == j1:
                                    index = 1
                                break
                        # 若m = 1， 说明已经完成了绕行，直接退出①
                        # 若m = 2， 说明pi2可以绕过，pi3不行，回退也无法绕行，直接退出①
                        # 若m = 3， 说明向右绕行成功，但是会撞上其他障碍物，直接退出①
                        if m == 1 or m == 2 or m == 3:
                            break
                else:  # 向左
                    for j in range(0, j1 - 2):
                        len_pi = math.sqrt((pi[1][0] - xyz[0][0]) ** 2 + (pi[1][1] - xyz[0][1]) ** 2 + (pi[1][2] - xyz[0][2]) ** 2)  # 管道终点到回退点的距离
                        new_pi = [pi[0], xyz[0], pi[2], pi[3]]  # 回退后的管道流
                        for i in range(1, 9):
                            a = qiudian(new_pi[0], new_pi[1], (i / 1) * wide, 1)
                            list11 = a[1].tolist()
                            pi2 = [new_pi[1], list11, pi[2], pi[3]]  # 向右拐（y方向）
                            test1 = pzjc([pi2], [column])
                            len_pi3 = 2 * d1 * (j + 1) + wide
                            if test1 == [[]]:
                                b = qiudian(pi2[0], pi2[1], len_pi3, 1)
                                list2 = b[0].tolist()
                                pi3 = [pi2[1], list2, pi[2], pi[3]]
                                c = qiudian(pi3[0], pi3[1], (i / 1) * wide, 1)
                                list22 = c[0].tolist()
                                pi4 = [pi3[1], list22, pi[2], pi[3]]
                                pi5 = [pi4[1], pi[1], pi[2], pi[3]]
                                a1 = pzjc([pi3, pi4, pi5], [column])
                                if a1 == [[], [], []]:
                                    m = 1
                                    final_pi = [new_pi, pi2, pi3, pi4, pi5]
                                    for z in range(0, len(block_list)):
                                        d = pzjc(final_pi, [block_list[z]])
                                        e = []
                                        for o in range(0, len(final_pi)):
                                            e.append([])
                                        if d == e:
                                            n = n + 1
                                        else:
                                            continue
                                    if n == len(block_list):  # 绕行完成
                                        break
                                    else:  # 向右绕行有障碍物
                                        m = 3
                                        if temp == 0:  # 第一次调用，尝试向右
                                            final_pi = Avoid_column(pi, column, block_list, BlockBase, 1)
                                            final_pi = [final_pi[0]]
     # 我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释我是注释
                                            # 测finalpi是不是2维（左边三个括号
                                            f = pzjc(final_pi, block_list)
                                            if len(block_list) == 0:
                                                break
                                            else:
                                                g = []
                                                for o in range(0, len(final_pi)):
                                                    g.append([])
                                                if f == g:
                                                    break
                                        else:  # 第n次调用，重新计算避障
                                            final_pi = Pi_bypass(final_pi, block_list, BlockBase)

                                else:
                                    if i == 8:
                                        m = 2
                                        index = 1
                                    continue
                            else:
                                xyz1 = Average_value([xyz[0]], pi)
                                xyz[0] = xyz1[0]
                                if j == j1:
                                    index = 1
                                break
                        if m == 1 or m == 2 or m == 3:
                            break

                    if m == 3 and temp == 0:
                        final_pi = Avoid_column(pi, column, block_list, BlockBase, 1)
                        final_pi = final_pi[0]
        else:
            index = 1
    if index == 1:  # 左右都有障碍物 从中穿过
        final_pi = pi
    rectify = [0, 0, 0]
    return [final_pi, rectify]



# temp: 障碍物的属性
# pi： 管道坐标
# stru: 障碍物表达式
# list1: 附近障碍物列表
def Avoid_obstacles(pi, stru, block_list, temp, BlockBase):
    if temp == 'Frame':
        new_pi1 = Avoid_crossbeam(pi, stru, block_list, temp, BlockBase)
        new_pi = new_pi1[0]
        rectify = new_pi1[1]
    elif temp == 'Column':
        new_pi1 = Avoid_column(pi, stru, block_list, BlockBase)
        new_pi = new_pi1[0]
        rectify = new_pi1[1]
    elif temp == 'Pipe':
        if abs(stru[1][2] - stru[0][2]) < 50:
            if abs(pi[1][2] - pi[0][2]) > 50:
                new_pi1 = Avoid_column(pi, stru, block_list, BlockBase)
                new_pi = new_pi1[0]
                rectify = new_pi1[1]
            else:
                new_pi1 = Avoid_crossbeam(pi, stru, block_list, temp, BlockBase)
                new_pi = new_pi1[0]
                rectify = new_pi1[1]
        else:
            new_pi1 = Avoid_column(pi, stru, block_list, BlockBase)
            new_pi = new_pi1[0]
            rectify = new_pi1[1]
    else:
        new_pi = pi
        rectify = [0, 0, 0]
    return [new_pi, rectify]


# 输入管道，障碍物列表，障碍物类型，输出避障后的管道
# 需完善：temp为障碍物类型，可考虑在函数中获取障碍物的类型，而不作为参数使用
def Pi_bypass(pi, block_list, BlockBase):
    if not block_list:
        print("no blocks")
        return pi
    try:
        for j in range(0, len(pi)):
            for i in range(0, len(block_list)):
                try:
                    a = pzjc([pi[j]], [block_list[i]])
                except:
                    print('pzjc函数异常，返回原管道')
                    return pi
                if a != [[]]:
                    type = check([block_list[i]], BlockBase)
                    block_list_leave = []
                    for z in range(1, len(block_list)):
                        block_list_leave.append(block_list[z])
                    try:
                        pi1 = Avoid_obstacles(pi[j], block_list[i], block_list_leave, type, BlockBase)
                        new_pi = pi1[0]
                        rectify = pi1[1]
                    except:
                        print('Avoid_obstacles异常')
                        return pi
                    if new_pi != pi[j]:
                        pi.pop(j)
                        for z in range(0, len(new_pi)):
                            pi.insert(j + z, new_pi[z])
                        for o in range(len(new_pi), len(pi)):
                            pi[o][0][0] = pi[o][0][0] + rectify[0]
                            pi[o][0][1] = pi[o][0][1] + rectify[1]
                            pi[o][0][2] = pi[o][0][2] + rectify[2]
                            pi[o][1][0] = pi[o][1][0] + rectify[0]
                            pi[o][1][1] = pi[o][1][1] + rectify[1]
                            pi[o][1][2] = pi[o][1][2] + rectify[2]
                        break
                    else:
                        break
                else:
                    continue
        return pi
    except:
        print('Pi_bypass异常，输出原管道')
        return pi

# list1 = [[[348593.065319341, 120641.158927369, 56900.0], [348593.065319341, 120641.158927369, 55568.9], 1100.0, 100.0], [[349243.865319341, 120641.158927369, 54918.1], [350693.065319341, 120641.158927369, 54918.1], 1200.0, 100.0]]
# pi = [[[350216.954417915, 120562.224099402, 56121.2], [347416.954417915, 120562.224099402, 56168.8], 100.0, 100.0]]
# newpi = Pi_bypass(pi, list1, BlockBase)


